#include "AP_PRLS.h"

AP_PRLS::AP_PRLS()
{

 first_exec = true;
 
 P1_p.push_back(0.0);
 P1_p.push_back(0.0);
 
 P2_p.push_back(0.0);
 P2_p.push_back(0.0);
 
 P1_c.push_back(0.0);
 P1_c.push_back(0.0);
 
 P2_c.push_back(0.0);
 P2_c.push_back(0.0);
 


}

void AP_PRLS::scan_polarTocartesian(TypeScan& scan,vector<GPoint2D>& cart_scan_)
{

   float cos_curangle, sin_curangle,cur_angle, dist;
   cart_scan_.resize(scan.nbPoints);
   
     for (int i = 0; i < scan.nbPoints; i++) {
    			
			cur_angle = scan.tabScan[i].angle * M_PI / 180;
			dist = scan.tabScan[i].distance;
			cos_curangle = cos(cur_angle);
			sin_curangle = sin(cur_angle);
			
		  cart_scan_[i].x = dist * cos_curangle;
		  cart_scan_[i].y = -dist * sin_curangle;

		  }

}

void AP_PRLS::analyse(TypeScan& scan)
{
    if (AP_DEBUG) cout << "void AP_PRLS::analyse" << endl;
    
    int i,j;
    int nb_pts;
    int end_process = 0;
    int next_line = 0;
    float x1,y1;
    float x2,y2;
    float d ;
    
    float K[2];
    float rho,phi,delta;
    
    float r1,r2;
    float theta1,theta2;
    
    float A,B;
    
    int n_pts_line;
    
    scan_polarTocartesian(scan,cart_scan);
    
     nb_pts = cart_scan.size();
     
     vec_A.clear();
     vec_B.clear();
     vec_dev.clear();
     
     line_deb.clear();
      angle_deb.clear();
      
      line_end.clear();
      angle_end.clear();
      
      P1_p[0] = 0;
      P1_p[1] = 0;
      P2_p[0] = 0;
      P2_p[1] = 0;
      
     nline = -1;
     
      i=0;
      
      while(!end_process)
      {
      
         /*  Initialisation with two points scan.tabScan[i].distance*/
         x1 = cart_scan[i].x;
         y1 = cart_scan[i].y;
         while(i<nb_pts && scan.tabScan[i].distance<0.2)
         {
          i++;
         } 
         cout<<"istar: "<<i<<" ang:"<<scan.tabScan[i].angle<<endl;
        if(i+1<nb_pts)
        {
            x1 = cart_scan[i].x;
            y1 = cart_scan[i].y;
            angle_deb.push_back(scan.tabScan[i].angle * M_PI / 180);
            
            if(scan.tabScan[i+1].distance<0.2)
            {
                cout<<"ff"<<endl;
                 if(i+2<nb_pts)
                 {
                    if(scan.tabScan[i+2].distance<0.2)
                    {
                    cout<<"ff2"<<endl;
                          i++;
                         continue;
                    }
                    else
                    {
                      cout<<"ff3"<<endl;
                      i++;
                    }
                 }
                 else
                 {
                  end_process = 1;
                  continue;
                 }
            }
            
            x2 = cart_scan[i+1].x;
            y2 = cart_scan[i+1].y;
           
           nline++;
           
           P1_p[0] = 0;
            P1_p[1] = 0;
            P2_p[0] = 0;
            P2_p[1] = 0;
             
           vec_A.push_back( (y1-y2)/(x2*y1-x1*y2));     
           vec_B.push_back( (x2-x1)/(x2*y1-x1*y2));
           A = vec_A[nline];
           B = vec_B[nline];
           d = (x1*A+y1*B-1)*(x1*A+y1*B-1)/(A*A+B*B) + (x2*A+y2*B-1)*(x2*A+y2*B-1)/(A*A+B*B) ;
           
           vec_dev.push_back(d);
            
            angle_end.push_back(scan.tabScan[i+1].angle * M_PI / 180);
           i++;
           
          n_pts_line = 2; 
          
          next_line = 0;
            
           while(!next_line)
           {
              i++;
              
              if(i<nb_pts)
              {
                  x1 = cart_scan[i].x;
                  y1 = cart_scan[i].y;
                  
                  A = vec_A[nline];
                  B = vec_B[nline];
                  
                  d = (x1*A+y1*B-1)*(x1*A+y1*B-1)/(A*A+B*B);
                  cout<<"d:"<<d<<endl;
                  if((d < 9*vec_dev[nline] && scan.tabScan[i].distance> 0.2) || (n_pts_line ==2 && scan.tabScan[i].distance> 0.2) )
                  {
                  
                      vec_dev[nline] = vec_dev[nline]+d;
                      
                      angle_end[nline]  = scan.tabScan[i].angle * M_PI / 180;
                      
                      P1_p[0] = P1_p[0] + x1*x1;
                      P1_p[1] = P1_p[1] + x1*y1;
                      
                      P2_p[0] = P2_p[0] + x1*y1;
                      P2_p[1] = P2_p[1] + y1*y1;
                      
                      delta = P1_p[0]*P2_p[1] - P2_p[0]*P1_p[1];
                      
                      K[0] = 1/delta*(x1*P2_p[1]-y1*P1_p[1]); 
                      K[1] = 1/delta*(y1*P1_p[0]-x1*P2_p[0]);
                      
                      vec_A[nline] = A + K[0]*(1-(x1*A+y1*B)); 
                      vec_B[nline] = B + K[1]*(1-(x1*A+y1*B)); 
                      
                      n_pts_line++;
                      
                  }
                  else
                  {
                    if(i+1<nb_pts)
                    {
                    cout<<"ttt : "<<scan.tabScan[i].distance<<endl;
                      if(scan.tabScan[i].distance< 0.2 && scan.tabScan[i+1].distance> 0.2)
                      {
                        cout<<"racc"<<endl;
                        next_line=0;
                      }
                      else
                      {
                        next_line=1;
                      }
                    }
                    else
                    {
                      next_line = 1;
                     }
                  }
                  
                 
                  
              }
              else
              {
                next_line =1;
                end_process= 1;
              }
                     
              
           }
           
            A = vec_A[nline];
            B = vec_B[nline];
            rho = 1/sqrt(A*A+B*B);
            phi = -atan2(B,A); 
            
            
            
            theta1 = angle_deb[nline];
            theta2 = angle_end[nline];
            
            r1  = rho/cos(phi-theta1);
            r2  = rho/cos(phi-theta2);
            cout<<"iend: "<<i<<"  rho:"<<rho<<"  phi: "<<phi*180.0/M_PI<<endl;
             cout<<"theta1:"<<theta1*180.0/M_PI<<"  theta2: "<<theta2*180.0/M_PI<<endl;
            
            line_deb.push_back(GPoint2D(r1*cos(theta1),r1*sin(theta1)));
            line_end.push_back(GPoint2D(r2*cos(theta2),r2*sin(theta2)));
           
        
        }
        else
        {
          end_process = 1;
        }
      
      
      }
       cout<<"nline: "<<nline<<endl;

  first_exec = false;

}


void AP_PRLS::scan_polarTocartesian2(TypeScan& scan,vector<GPoint2D>& cart_scan_)
{

   float cos_curangle, sin_curangle,cur_angle, dist;
   int k=-1;
     cart_scan_.clear();
     id_scan.clear();
   
     for (int i = 0; i < scan.nbPoints; i++) {
    			
  			cur_angle = scan.tabScan[i].angle * M_PI / 180;
  			dist = scan.tabScan[i].distance;
  			if(dist>0.2)
  			{
  			   cos_curangle = cos(cur_angle);
  			   sin_curangle = sin(cur_angle);
  			
  			    k++;
  			    cart_scan_.push_back( GPoint2D(0.0,0.0));
  			    id_scan.push_back(i);
  		     cart_scan_[k].x = dist * cos_curangle;
  		     cart_scan_[k].y = -dist * sin_curangle;
        }
		  }

}


void AP_PRLS::analyse2(TypeScan& scan)
{
    if (AP_DEBUG) cout << "void AP_PRLS::analyse" << endl;
    
    int i,j;
    int nb_pts;
    int end_process = 0;
    int next_line = 0;
    float x1,y1;
    float x2,y2;
    float xp,yp;
    float d ;
    
    float K[2];
    float rho,phi,delta;
    
    float r1,r2;
    float theta1,theta2;
    
    float A,B;
    
    int n_pts_line;
    
    scan_polarTocartesian2(scan,cart_scan);
    
     nb_pts = cart_scan.size();
     
     vec_A.clear();
     vec_B.clear();
     vec_dev.clear();
     
     line_deb.clear();
      angle_deb.clear();
      
      line_end.clear();
      angle_end.clear();
      
      P1_p[0] = 0;
      P1_p[1] = 0;
      
      P2_p[0] = 0;
      P2_p[1] = 0;
      
     nline = -1;
     
      i=0;
      if(nb_pts==0)
      {
          return;
      }
       xp = cart_scan[0].x;
       yp = cart_scan[0].y;
      while(!end_process)
      {
      
         /*  Initialisation with two points scan.tabScan[i].distance*/
         x1 = cart_scan[i].x;
         y1 = cart_scan[i].y;
         cout<<"istar: "<<i<<" ang:"<<scan.tabScan[id_scan[i]].angle<<endl;
        if(i+1<nb_pts)
        {
            x1 = cart_scan[i].x;
            y1 = cart_scan[i].y;
            angle_deb.push_back(scan.tabScan[id_scan[i]].angle * M_PI / 180);

            x2 = cart_scan[i+1].x;
            y2 = cart_scan[i+1].y;
           
           nline++;
           
           P1_p[0] = 0;
            P1_p[1] = 0;
            P2_p[0] = 0;
            P2_p[1] = 0;
             
           vec_A.push_back( (y1-y2)/(x2*y1-x1*y2));     
           vec_B.push_back( (x2-x1)/(x2*y1-x1*y2));
           A = vec_A[nline];
           B = vec_B[nline];
           d = (x1*A+y1*B-1)*(x1*A+y1*B-1)/(A*A+B*B) + (x2*A+y2*B-1)*(x2*A+y2*B-1)/(A*A+B*B) ;
           
           vec_dev.push_back(d);
            
            angle_end.push_back(scan.tabScan[id_scan[i+1]].angle * M_PI / 180);
           i++;
           
          n_pts_line = 2; 
          
          next_line = 0;
            
           while(!next_line)
           {
              i++;
              
              if(i<nb_pts)
              {
                  x1 = cart_scan[i].x;
                  y1 = cart_scan[i].y;
                  
                  A = vec_A[nline];
                  B = vec_B[nline];
                  
                  d = (x1*A+y1*B-1)*(x1*A+y1*B-1)/(A*A+B*B);
                  cout<<"d:"<<d<<endl;
                  if(((d < 9*vec_dev[nline])  || (n_pts_line <4) )&& (x1-xp)*(x1-xp)+(y1-yp)*(y1-yp)<0.2*0.2)
                  {
                  
                      vec_dev[nline] = vec_dev[nline]+d;
                      
                      angle_end[nline]  = scan.tabScan[id_scan[i]].angle * M_PI / 180;
                      
                      P1_p[0] = P1_p[0] + x1*x1;
                      P1_p[1] = P1_p[1] + x1*y1;
                      
                      P2_p[0] = P2_p[0] + x1*y1;
                      P2_p[1] = P2_p[1] + y1*y1;
                      
                      delta = P1_p[0]*P2_p[1] - P2_p[0]*P1_p[1];
                      
                      K[0] = 1/delta*(x1*P2_p[1]-y1*P1_p[1]); 
                      K[1] = 1/delta*(y1*P1_p[0]-x1*P2_p[0]);
                      
                      vec_A[nline] = A + K[0]*(1-(x1*A+y1*B)); 
                      vec_B[nline] = B + K[1]*(1-(x1*A+y1*B)); 
                      
                      n_pts_line++;
                      
                  }
                  else
                  {
                 
                      next_line = 1;
                     
                  }
                  
                 
                  
              }
              else
              {
                next_line =1;
                end_process= 1;
              }
              xp = x1;
              yp = y1;       
              
           }
            
            
            A = vec_A[nline];
            B = vec_B[nline];
            rho = 1/sqrt(A*A+B*B);
            phi = -atan2(B,A); 
            
            
            
            theta1 = angle_deb[nline];
            theta2 = angle_end[nline];
            
            r1  = rho/cos(phi-theta1);
            r2  = rho/cos(phi-theta2);
            cout<<"iend: "<<i<<"  rho:"<<rho<<"  phi: "<<phi*180.0/M_PI<<endl;
             cout<<"theta1:"<<theta1*180.0/M_PI<<"  theta2: "<<theta2*180.0/M_PI<<endl;
            
            line_deb.push_back(GPoint2D(r1*cos(theta1),r1*sin(theta1)));
            line_end.push_back(GPoint2D(r2*cos(theta2),r2*sin(theta2)));
              
              
              if(n_pts_line<4)
            {
                nline--;
                angle_deb.pop_back();
                angle_end.pop_back();
                vec_A.pop_back();
                vec_B.pop_back();
                vec_dev.pop_back();
                line_deb.pop_back();
                line_end.pop_back();
            }
        
        }
        else
        {
          end_process = 1;
        }
      
      
      }
       cout<<"nline: "<<nline<<endl;

  first_exec = false;

}